#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
传感器数据融合模块

负责处理和融合来自不同传感器的数据，提供统一的环境感知接口。
"""

from typing import Dict, Any, List, Optional
import numpy as np
from datetime import datetime


class SensorFusion:
    """传感器数据融合类"""

    def __init__(self):
        """初始化传感器数据融合器"""
        # 传感器数据缓存
        self.sensor_data = {
            "imu": {
                "acceleration": np.zeros(3),
                "angular_velocity": np.zeros(3),
                "orientation": np.zeros(3),
                "timestamp": None,
            },
            "ultrasonic": {
                "distances": np.zeros(6),  # 6个超声波传感器
                "timestamp": None,
            },
            "vision": {
                "obstacles": [],  # 障碍物列表
                "targets": [],  # 目标列表
                "timestamp": None,
            },
        }

        # 数据有效性标志
        self.data_valid = {"imu": False, "ultrasonic": False, "vision": False}

        # 数据融合参数
        self.fusion_params = {
            "max_age": 0.5,  # 数据最大有效期（秒）
            "min_confidence": 0.5,  # 最小置信度
            "update_rate": 0.1,  # 更新率（秒）
        }

    def update_imu_data(
        self,
        acceleration: np.ndarray,
        angular_velocity: np.ndarray,
        orientation: np.ndarray,
    ) -> None:
        """
        更新IMU数据

        Args:
            acceleration: 加速度数据 [x, y, z]
            angular_velocity: 角速度数据 [x, y, z]
            orientation: 姿态数据 [roll, pitch, yaw]
        """
        self.sensor_data["imu"].update(
            {
                "acceleration": np.array(acceleration),
                "angular_velocity": np.array(angular_velocity),
                "orientation": np.array(orientation),
                "timestamp": datetime.now(),
            }
        )
        self.data_valid["imu"] = True

    def update_ultrasonic_data(self, distances: np.ndarray) -> None:
        """
        更新超声波数据

        Args:
            distances: 距离数据数组 [d1, d2, ..., d6]
        """
        self.sensor_data["ultrasonic"].update(
            {"distances": np.array(distances), "timestamp": datetime.now()}
        )
        self.data_valid["ultrasonic"] = True

    def update_vision_data(
        self, obstacles: List[Dict[str, Any]], targets: List[Dict[str, Any]]
    ) -> None:
        """
        更新视觉数据

        Args:
            obstacles: 障碍物列表
            targets: 目标列表
        """
        self.sensor_data["vision"].update(
            {"obstacles": obstacles, "targets": targets, "timestamp": datetime.now()}
        )
        self.data_valid["vision"] = True

    def get_fused_data(self) -> Dict[str, Any]:
        """
        获取融合后的数据

        Returns:
            Dict[str, Any]: 融合后的数据
        """
        # 检查数据有效性
        self._check_data_validity()

        # 融合数据
        fused_data = {
            "timestamp": datetime.now(),
            "robot_state": self._get_robot_state(),
            "environment": self._get_environment_state(),
            "confidence": self._calculate_confidence(),
        }

        return fused_data

    def _check_data_validity(self) -> None:
        """检查数据有效性"""
        current_time = datetime.now()

        for sensor_type in self.data_valid:
            if self.sensor_data[sensor_type]["timestamp"] is None:
                self.data_valid[sensor_type] = False
                continue

            age = (
                current_time - self.sensor_data[sensor_type]["timestamp"]
            ).total_seconds()
            if age > self.fusion_params["max_age"]:
                self.data_valid[sensor_type] = False

    def _get_robot_state(self) -> Dict[str, Any]:
        """
        获取机器人状态

        Returns:
            Dict[str, Any]: 机器人状态
        """
        if not self.data_valid["imu"]:
            return {"status": "unknown"}

        return {
            "status": "active",
            "acceleration": self.sensor_data["imu"]["acceleration"].tolist(),
            "angular_velocity": self.sensor_data["imu"]["angular_velocity"].tolist(),
            "orientation": self.sensor_data["imu"]["orientation"].tolist(),
        }

    def _get_environment_state(self) -> Dict[str, Any]:
        """
        获取环境状态

        Returns:
            Dict[str, Any]: 环境状态
        """
        environment = {"obstacles": [], "targets": [], "distances": None}

        # 添加视觉数据
        if self.data_valid["vision"]:
            environment["obstacles"] = self.sensor_data["vision"]["obstacles"]
            environment["targets"] = self.sensor_data["vision"]["targets"]

        # 添加超声波数据
        if self.data_valid["ultrasonic"]:
            environment["distances"] = self.sensor_data["ultrasonic"][
                "distances"
            ].tolist()

        return environment

    def _calculate_confidence(self) -> float:
        """
        计算数据置信度

        Returns:
            float: 置信度值（0-1）
        """
        # 计算有效数据比例
        valid_count = sum(1 for valid in self.data_valid.values() if valid)
        total_count = len(self.data_valid)

        # 计算时间衰减因子
        current_time = datetime.now()
        time_factors = []

        for sensor_type, data in self.sensor_data.items():
            if data["timestamp"] is not None:
                age = (current_time - data["timestamp"]).total_seconds()
                time_factor = max(0, 1 - age / self.fusion_params["max_age"])
                time_factors.append(time_factor)

        # 计算最终置信度
        if not time_factors:
            return 0.0

        time_confidence = np.mean(time_factors)
        data_confidence = valid_count / total_count

        return min(1.0, time_confidence * data_confidence)
